/*
 * Climber.cpp
 *
 *  Created on: Dec 27, 2012
 *      Author: trwulff
 */

#include "Climber.hpp"




/*
 * Default constructor
 */
Climber::Climber() :
	motorJaguar(CLIMBER_MOTOR_ID, CANJaguar::kPercentVbus),
	clampSolenoid(CLIMBER_CLAMP_SOLENOID_ID),
	motorSpeed(0)
{

	printf("%s %s %s\n",__FILE__,__DATE__,__TIME__);

	 motorJaguar.EnableControl();
	 motorJaguar.SetExpiration(30);
	 motorJaguar.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake);
	 motorJaguar.SetSafetyEnabled(false);
	 this->SetSpeed(0);
	 this->ReleaseClamp();
	 SmartDashboard::init();
	 SmartDashboard::PutBoolean("Climber Enable",false);
	 SmartDashboard::PutBoolean("Climber Clamp",false);
	 SmartDashboard::PutNumber("Climber Speed",.5);
	SmartDashboard::PutBoolean("Climb Cal On",false);
	SmartDashboard::PutBoolean("C Check",false);

}

/*
 * Default destructor
 */
 Climber::~Climber()
{

}

void Climber::Update()
// The update function is expected for every class and must be called each cycle of the command loop.
// This function sould contain any motor calls. This will keep the motors functioning.
{
	motorJaguar.EnableControl();
	motorJaguar.Set(this->motorSpeed);
}


void Climber::SetSpeed(double speed)
{
	if (motorJaguar.GetControlMode() == CANJaguar::kPercentVbus)
	{
		if (speed < -1.0)
		{
			speed = -1.0;
		}
	
		if (speed > 1.0)
		{
			speed = 1.0;
		}
	}
	motorSpeed = speed;
}

void Climber::ApplyClamp()
{
	this->clampSolenoid.Set(true);
	SmartDashboard::PutBoolean("Clamp Applied",true);
}

void Climber::ReleaseClamp()
{
	this->clampSolenoid.Set(false);
	SmartDashboard::PutBoolean("Clamp Applied",false);
}

bool Climber::Calibrate()
{
	SmartDashboard::PutBoolean("C Check",true);
	if(SmartDashboard::GetBoolean("Climber Enable"))
	{
		SmartDashboard::PutBoolean("Climb Cal On",true);
		Climber::SetSpeed(SmartDashboard::GetNumber("Climber Speed"));
		Climber::Update();

		 if	 (SmartDashboard::GetBoolean("Climber Clamp"))
		 {
			 Climber::ApplyClamp();
		 }
		 else
		 {
			 Climber::ReleaseClamp();
		 }

		 return true;
	}
	SmartDashboard::PutBoolean("Climb Cal On",false);

	return false;
	
}

